#include <geometry_msgs/PoseStamped.h>
#include <ros/ros.h>
#include <gflags/gflags.h>
#include <nav_msgs/Odometry.h>
#include <functional>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <chrono>
#include <sophus/so3.hpp>
#include <std_msgs/Float64.h>
#include <fstream>

DEFINE_string(true_topic, "/vrpn_client_node/uav1/pose", "This is dong pu true pose");
DEFINE_string(odom_topic, "/baton/stereo3/odometry", "This is odom pose");
DEFINE_double(gaptime, 1, "calculate gap time");

double all_error = 0.0;
double count = 0;

Eigen::Vector3d true_pose = Eigen::Vector3d::Zero();
Eigen::Vector3d ltrue_pose = Eigen::Vector3d::Zero();

Eigen::Vector3d odom_pose = Eigen::Vector3d::Zero();
Eigen::Vector3d lodom_pose = Eigen::Vector3d::Zero();

Eigen::Matrix3d true_R = Eigen::Matrix3d::Identity();
Eigen::Matrix3d ltrue_R = Eigen::Matrix3d::Identity();

Eigen::Matrix3d odom_R = Eigen::Matrix3d::Identity();
Eigen::Matrix3d lodom_R = Eigen::Matrix3d::Identity();

bool flag = false;
bool firsttrue = true;
bool firstodom = true;
ros::Time time1;
ros::Time time2;

ros::Publisher rotation_error_pub;
ros::Publisher curr_error_pub;
ros::Publisher all_error_pub;

void dongpucallback(const geometry_msgs::PoseStamped::ConstPtr& pose) {
    // 处理 true pose 数据
    double x = pose->pose.position.x;
    double y = pose->pose.position.y;
    double z = pose->pose.position.z;

    double qw = pose->pose.orientation.w;
    double qx = pose->pose.orientation.x;
    double qy = pose->pose.orientation.y;
    double qz = pose->pose.orientation.z;

    true_pose << x, y, z;
    Eigen::Quaterniond q(qw, qx, qy, qz);
    q.normalize();
    true_R = q.toRotationMatrix();
    if (firsttrue) {
        ROS_WARN("Received first true pose data!");
        firsttrue = false;
        time1 = ros::Time::now();
        ltrue_pose = true_pose;
        ltrue_R = true_R;
        return;
    }

    ros::Time nowtime = ros::Time::now();
    double t = (nowtime - time1).toSec();
    if (t >= FLAGS_gaptime) {
        time2 = nowtime;
        flag = true;
        count++;
    }
}

void odomcallback(const nav_msgs::Odometry::ConstPtr& pose) {
    // 处理 odom pose 数据

    // 旋转矩阵RI2C
    Eigen::Matrix3d  RI2C;
    RI2C = Eigen::AngleAxisd(M_PI * 0.5,Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(- M_PI * 0.5,Eigen::Vector3d::UnitZ());

    double x = pose->pose.pose.position.x;
    double y = pose->pose.pose.position.y;
    double z = pose->pose.pose.position.z;

    double qw = pose->pose.pose.orientation.w;
    double qx = pose->pose.pose.orientation.x;
    double qy = pose->pose.pose.orientation.y;
    double qz = pose->pose.pose.orientation.z;

    odom_pose << x, y, z;
    Eigen::Quaterniond q(qw, qx, qy, qz);
    q.normalize();
    odom_R = RI2C * q.toRotationMatrix();
    // odom_R = q.toRotationMatrix();
    if (firstodom) {
        ROS_WARN("Received first true odom data!");
        firstodom = false;
        lodom_pose = odom_pose;
        lodom_R = odom_R;
        return;
    }

    ros::Time currenttime = ros::Time::now();
    if (flag) {
        ROS_WARN("Received odom pose data!");
        // 计算误差
        // 1. 平移误差
        double t = (time1 - currenttime).toSec();
        double delt = (true_pose - ltrue_pose).norm();

        double vx = pose->twist.twist.linear.x;
        double vy = pose->twist.twist.linear.y;
        double vz = pose->twist.twist.linear.z;

        double wx = pose->twist.twist.angular.x;
        double wy = pose->twist.twist.angular.y;
        double wz = pose->twist.twist.angular.z;

        odom_pose = odom_pose + t * Eigen::Vector3d(vx, vy, vz);
        double odelt = (odom_pose - lodom_pose).norm();

        double trans_error = (odelt - delt) * (odelt - delt);
        ROS_WARN("The translate error is :%f", trans_error);

        // 2. 旋转误差
        Sophus::SO3d delR = Sophus::SO3d(ltrue_R).inverse() * Sophus::SO3d(true_R);
        Eigen::Quaterniond delta_q(1, wx * 1 / 2 * t, wy * 1 / 2 * t, wz * 1 / 2 * t);
        q = q * delta_q;
        q.normalize();
        odom_R = q.toRotationMatrix();
        Sophus::SO3d odelR = Sophus::SO3d(lodom_R).inverse() * Sophus::SO3d(odom_R);
        Sophus::SO3d error_R = odelR.inverse() * delR;
        Eigen::Vector3d error_axis_angle = error_R.log();
        double rotation_error = error_axis_angle.norm();
        ROS_WARN("The transform error is : %f", rotation_error);

        // 更新总误差
        all_error = all_error * (count - 1) / (count) + (trans_error + rotation_error) / count;
        ROS_WARN("The all error is : %f", all_error);

        // 发布误差数据
        std_msgs::Float64 rotation_error_msg;
        rotation_error_msg.data = rotation_error;
        rotation_error_pub.publish(rotation_error_msg);

        std_msgs::Float64 curr_error_msg;
        curr_error_msg.data = trans_error;
        curr_error_pub.publish(curr_error_msg);

        std_msgs::Float64 all_error_msg;
        all_error_msg.data = all_error;
        all_error_pub.publish(all_error_msg);

        // 更新状态
        lodom_pose = odom_pose;
        lodom_R = odom_R;
        ltrue_pose = true_pose;
        ltrue_R = true_R;
        time1 = time2;
        flag = false;
    }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "trajectory_err");
    gflags::ParseCommandLineFlags(&argc, &argv, true);
    ros::NodeHandle nh;

    // 定义发布者
    rotation_error_pub = nh.advertise<std_msgs::Float64>("/rotation_error", 10);
    curr_error_pub = nh.advertise<std_msgs::Float64>("/trans_error", 10);
    all_error_pub = nh.advertise<std_msgs::Float64>("/all_error", 10);

    ROS_WARN("evo start ");
    ros::Subscriber dongpu = nh.subscribe(FLAGS_true_topic, 10, dongpucallback);
    ros::Subscriber odompose = nh.subscribe(FLAGS_odom_topic, 10, odomcallback);

    ros::spin();

    return 0;
}